/*
 * CatGraph.h
 *
 *  Created on: 10/11/2011
 *      Author: will
 */

#ifndef CATGRAPH_H_
#define CATGRAPH_H_

#include "GraphNode.h"
#include "GraphEdge.h"
#include "Particle.h"
#include "LocalGraph.h"

#include "MotionModel.h"

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/nvp.hpp>

#include "../bagofwords/BagOfWords.h"
#include "../fabmap/ChowLiuTree.h"

#include <vector>
#include <map>
#include <utility>
#include <iostream>
#include <fstream>
#include <eigen3/Eigen/Core>
#include <opencv2/opencv.hpp>
#include <mex.h>
#include <string.h>

using std::vector;
using std::multimap;
using std::string;
using std::ifstream;
using std::ofstream;
using cv::Mat;
using cv::Ptr;
using cv::DescriptorMatcher;

namespace boost {
namespace serialization {
template<class Archive, typename _Scalar, int _Rows, int _Cols>
inline void save(Archive & ar, Eigen::Matrix<_Scalar, _Rows, _Cols> const &t,
		const unsigned int file_version) {
	int rows = t.rows(), cols = t.cols();
	ar << make_nvp("rows", rows) << make_nvp("cols", cols);
	ar << make_nvp("data", make_array(&t(0), t.size()));
}

template<class Archive, typename _Scalar, int _Rows, int _Cols>
inline void load(Archive & ar, Eigen::Matrix<_Scalar, _Rows, _Cols> &t,
		const unsigned int file_version) {
	int rows, cols;
	ar >> make_nvp("rows", rows) >> make_nvp("cols", cols);
	t.resize(rows, cols);
	ar >> make_nvp("data", make_array(&t(0), t.size()));
}

template<class Archive, typename _Scalar, int _Rows, int _Cols>
inline void serialize(Archive & ar, Eigen::Matrix<_Scalar, _Rows, _Cols> &t,
		const unsigned int file_version) {
	split_free(ar, t, file_version);
}

} // namespace serialization
} // namespace boost

class CatGraph {
public:
	CatGraph();
	virtual ~CatGraph();
	CatGraph(const CatGraph & other);
	CatGraph & operator=(const CatGraph & other);
	void serializeGraph(string fileName);
	void deserializeGraph(string fileName);

	// Graph Parameters
	unsigned int frameNum;
	unsigned int numParticles;
	unsigned int numNodes;
	Matrix3d Q;
	Vector3d u_mean;
	double odoLikelihood_mean;
	double visLikelihood_mean;
	double w_mean;

	// Visual Parameters
	Mat codebook;
	cv::Ptr<cv::DescriptorMatcher> matcher;
	ClTree clTree;
	FastLookupFabMap fabMap;
	BowTemplate z_avg;

	// Data Storage
	vector<GraphNode*> nodes;
	vector<GraphEdge*> edges;
	vector<Particle*> particles;
	multimap<double,GraphNode*> information;
	map<GraphNode*,multimap<double,GraphNode*>::iterator > informationLookup;
	list<GraphNode*> updateList;

	// Functions

	CatGraph(unsigned int numParticles, unsigned int numNodes,
			Matrix3d Q, Vector3d u_mean);

	void initialiseVision(string codebookPath, string chowLiuPath);

	void initialiseTrajectory(Mat* observation, string descriptorPath);

	void initialiseParticles(unsigned int blankFrames);

	void addNode(Mat* observation, string descriptorPath, Vector3d odometry);

	void updateInformation();

	void pruneNode(GraphNode* node);

	void updateParticlePositions(Vector3d odometry, unsigned int blankFrames);

	void updateParticleWeights();

	double calculateESS();

	void calculateHypothesis(double hypothesisDist, unsigned int blankFrames);

	void addLoopClosure(Particle* particle);

	void resampleParticles(double spreadFraction, double reverseFraction,
			unsigned int blankFrames);

	void buildCodebookFromMap(unsigned int numWords, string codebookPath);
	void buildChowLiuFromMap(double infoThreshold, string chowLiuPath);

	void recomputeWords();
	void recomputeLikelihoods();
	void recomputeInformation();


private:

	// Helper Functions

	void clearParticleReferences();

	Particle* randomParticle(unsigned int blankFrames);

	// Serialization Functions

	friend class boost::serialization::access;
	template<class Archive>
	void serialize(Archive &ar, const unsigned int version) {

		mexPrintf("SERIALIZATION: Processing CatGraph\n");

		// Parameters
		ar & BOOST_SERIALIZATION_NVP(frameNum);
		ar & BOOST_SERIALIZATION_NVP(numParticles);
		ar & BOOST_SERIALIZATION_NVP(numNodes);
		ar & BOOST_SERIALIZATION_NVP(Q);
		ar & BOOST_SERIALIZATION_NVP(u_mean);
		//ar & BOOST_SERIALIZATION_NVP(odoLikelihood_mean);
		//ar & BOOST_SERIALIZATION_NVP(visLikelihood_mean);
		//ar & BOOST_SERIALIZATION_NVP(w_mean);

		// Data Storage
		ar & BOOST_SERIALIZATION_NVP(nodes);
		ar & BOOST_SERIALIZATION_NVP(edges);
		ar & BOOST_SERIALIZATION_NVP(particles);
		//ar & BOOST_SERIALIZATION_NVP(information);

	}



};



#endif /* CATGRAPH_H_ */
